/*
 * state_machine.cpp
 *
 *  Created on: Jan 11, 2013
 *      Author: michiel
 */

#include "rp_states/state_machine.h"

#define CLIP(x, l, u) ((x)<(l))?(l):((x)>(u))?(u):(x)

void StateMachine::setState(STATE state) {
	_currentState = state;
}

STATE StateMachine::getState() {
	return _currentState;
}

void StateMachine::init() {
	ROS_INFO("Waiting for Ball Drop Server...");
	ballDropper_.waitForServer();
	ROS_INFO("Waiting for Marker Finder Server...");
	markerFinder_.waitForServer();
	ROS_INFO("Waiting for Positioner Server...");
	positioner_.waitForServer();
	ROS_INFO("Waiting for Ball Grabber Server...");
	ballGrabber_.waitForServer();
	ROS_INFO("Waiting for Idler Server...");
	idler_.waitForServer();
	ROS_INFO("Waiting for Drive Server...");
	driver_.waitForServer();
	ROS_INFO("Waiting for Manual Control Server...");
	manualController_.waitForServer();


	ROS_INFO("Listening to topics...");
	wheels_sub_		= nh_.subscribe(Topic::WHEELS_STATUS, 1, &StateMachine::wheelCallback, this);
	controller_sub_ = nh_.subscribe(Topic::PS3_JOY, 1, &StateMachine::controllerCallback, this);

	ROS_INFO("Publishing to topics...");
	emo_pub_		= nh_.advertise<comm::emotion>(Topic::EMO_STATE, 1);

	// TODO Init marker IDs;
	marker_ball_ = 0;
	marker_basket_ = 1;

	// Start idler server.
	rp_states::IdleGoal goal;
	goal.idle_duration = ros::Duration(5.0);
	idler_busy_ = true;
	idler_.sendGoal(goal,
			boost::bind(&StateMachine::idlerDoneCallback, this, _1, _2),
			boost::bind(&StateMachine::idlerActiveCallback, this));
}

void StateMachine::wheelCallback(const geometry_msgs::Twist::ConstPtr &msg) {
	wheels_speed_	= *msg;
}

void StateMachine::controllerCallback(const sensor_msgs::Joy::ConstPtr &msg) {
	BUTTONS buttons	= msg->buttons;
	AXES axes		= msg->axes;

	if (buttons[PS3_BUTTON_ACTION_CROSS] == 1) {
		button_grab_pressed_ = true;
	} else {
		button_grab_pressed_ = false;
	}

	if (buttons[PS3_BUTTON_ACTION_CIRCLE] == 1) {
		button_cancel_pressed_ = true;
	} else {
		button_cancel_pressed_ = false;
	}

	if (buttons[PS3_BUTTON_ACTION_TRIANGLE] == 1) {
		button_drop_pressed_ = true;
	} else {
		button_drop_pressed_ = false;
	}

	if (buttons[PS3_BUTTON_START] == 1) {
		button_start_pressed_ = true;
	} else {
		button_start_pressed_ = false;
	}

	if (buttons[PS3_BUTTON_SELECT] == 1) {
		manual_mode_ = true;
	} else {
		manual_mode_ = false;
	}
}

void StateMachine::spin() {
	ros::Rate loop_rate(10);

	while(ros::ok()) {
		// Switch state if needed.
		stateSwitcher();
		// Execute current state.
		stateHandler();
		emotionHandler();

		loop_rate.sleep();
		ros::spinOnce();
	}
}

void StateMachine::emotionHandler() {
	if (ros::Time::now() > emoTime_) {
		emomsg.state = false;
	}
	emo_pub_.publish(emomsg);
}
void StateMachine::prepareEmotion(int state, ros::Duration time) {
	ROS_INFO("Preparing emotion: %i", state);
	emoTime_ = ros::Time::now() + time;
	emomsg.state = true;
	emomsg.emotion = state;
}
void StateMachine::idlerDoneCallback(const GOALSTATE &state, const rp_states::IdleResultConstPtr& result) {
	ROS_INFO_NAMED("Idler", "Idler: DONE!");
	idler_busy_ = false;
	idle_result_ = *result;
}
void StateMachine::idlerActiveCallback() {
	ROS_INFO_NAMED("Idler", "Idler: BUSY!");
	idler_busy_ = true;
}
void StateMachine::driverDoneCallback(const GOALSTATE &state, const rp_states::DriveResultConstPtr &result) {
	ROS_INFO_NAMED("Driver", "Driver: DONE!");
	driver_busy_ = false;
	drive_result_ = *result;
}
void StateMachine::driverActiveCallback() {
	ROS_INFO_NAMED("Driver", "Driver: BUSY!");
	driver_busy_ = true;
}
void StateMachine::markerFinderDoneCallback(const GOALSTATE &state, const rp_states::FindMarkerResultConstPtr &result) {
	ROS_INFO_NAMED("Finder", "Finder: DONE!");
	finder_busy_ = false;
	find_result_ = *result;
}
void StateMachine::markerFinderActiveCallback() {
	ROS_INFO_NAMED("Finder", "Finderrun: BUSY!");
	finder_busy_ = true;
}
void StateMachine::positionerDoneCallback(const GOALSTATE &state, const rp_states::PositionTargetResultConstPtr &result) {
	ROS_INFO_NAMED("Positioner", "Positioner: DONE!");
	positioner_busy_ = false;
	position_result_ = *result;
}
void StateMachine::positionerActiveCallback() {
	ROS_INFO_NAMED("Positioner", "Positioner: BUSY!");
	positioner_busy_ = true;
}
void StateMachine::ballGrabberDoneCallback(const GOALSTATE &state, const rp_states::GrabBallResultConstPtr &result) {
	ROS_INFO_NAMED("Grabber", "Grabber: DONE!");
	grabber_busy_ = false;
	grab_result_ = *result;
}
void StateMachine::ballGrabberActiveCallback() {
	ROS_INFO_NAMED("Grabber", "Grabber: BUSY!");
	grabber_busy_ = true;
}
void StateMachine::ballDropperDoneCallback(const GOALSTATE &state, const rp_states::DropBallResultConstPtr &result) {
	ROS_INFO_NAMED("Dropper", "Dropper: DONE!");
	dropper_busy_ = false;
	drop_result_ = *result;
}
void StateMachine::ballDropperActiveCallback() {
	ROS_INFO_NAMED("Dropper", "Dropper: BUSY!");
	dropper_busy_ = true;
}
void StateMachine::manualControlDoneCallback(const GOALSTATE &state, const rp_states::ManualControlResultConstPtr &result) {
	ROS_INFO_NAMED("Manual", "Manual: DONE!");
	manual_busy_ = false;
	manual_result_ = *result;
}
void StateMachine::manualControlActiveCallback() {
	ROS_INFO_NAMED("Manual", "Manual: BUSY!");
	manual_busy_ = true;
}

void StateMachine::goalCanceler() {
	// Totally not winning any beauty contests, but works.
	idler_.cancelAllGoals();
	driver_.cancelAllGoals();
	positioner_.cancelAllGoals();
	ballDropper_.cancelAllGoals();
	ballGrabber_.cancelAllGoals();
	markerFinder_.cancelAllGoals();
}

void StateMachine::stateHandler() {
	switch(getState()) {
	case(IDLE): {
		prepareIdle();
		break;
	}
	case(MOVING_WITHOUT_BALL): {
		prepareDrive(false);
		break;
	}
	case(MOVING_WITH_BALL): {
		prepareDrive(true);
		break;
	}
	case(EMOTING): {
		break;
	}
	case(POSITIONING): {
		preparePositionTarget();
		break;
	}
	case(GRABBING): {
		prepareGrabBall();
		break;
	}
	case(RELEASING): {
		prepareBallDrop();
		break;
	}
	case(SEARCHING): {
		prepareFindMarker();
		break;
	}
	case(MANUAL): {
		prepareManual();
		break;
	}
	default: {
		ROS_INFO("EPIC DERP STATE, NO CLUE WTF HAPPENING!!! LOLOLOL!");
		break;
	}
	}
}

void StateMachine::stateSwitcher() {
	switch(getState()) {
	case(IDLE): {
		if (manual_mode_) {
			ROS_INFO("Idle -> Manual Control mode!");
			setState(MANUAL);
		} else if (button_start_pressed_) {
			ROS_INFO("Idle -> Moving without ball!");
			setState(MOVING_WITHOUT_BALL);
		} else if (idler_.getState() == GOALSTATE::SUCCEEDED) {
			ROS_INFO("Idle -> Emoting!");
			setState(EMOTING);
			prepareEmotion(idle_result_.action_selection, ros::Duration(4.0));
		}
		break;
	}
	case(MOVING_WITHOUT_BALL): {
		if (button_grab_pressed_) {
			ROS_INFO("Moving without ball -> Positioning to grab ball!");
			setPositionerTarget(marker_ball_);
			setState(POSITIONING);
		} else if (button_cancel_pressed_) {
			ROS_INFO("Moving without ball -> Idle!" );
			setState(IDLE);
		}
		break;
	}
	case(MOVING_WITH_BALL): {
		if (button_drop_pressed_) {
			ROS_INFO("Moving with ball -> Positiong to drop ball!");
			setPositionerTarget(marker_basket_);
			setState(POSITIONING);
		} else if (button_cancel_pressed_) {
			ROS_INFO("Moving with ball -> Idle!" );
			setState(IDLE);
		}
		break;
	}
	case(EMOTING): {
		if (button_start_pressed_) {
			ROS_INFO("Emoting -> Moving without ball!");
			comm::emotion msg;
			msg.state = false;
			emo_pub_.publish(msg);
			setState(MOVING_WITHOUT_BALL);
		} else if (ros::Time::now() > emoTime_) {
			ROS_INFO("Emoting -> Idle!");
			setState(IDLE);
		}
		break;
	}
	case(POSITIONING): {
		if (button_cancel_pressed_) {
			ROS_INFO("Positioning -> Moving without ball!");
			setState(MOVING_WITHOUT_BALL);
		} else if (positioner_.getState() == GOALSTATE::ABORTED) {
			// Can't find any marker lol.
			ROS_INFO("Positioning -> Finding marker!");
			setState(SEARCHING);
		} else if (positioner_.getState() == GOALSTATE::SUCCEEDED && positioner_.getResult()->at_location) {
			ROS_INFO("Positioning -> Grabbing ball!(NOT I GOT COMMENTED)");
			setState(GRABBING);
		} else if (positioner_.getState() == GOALSTATE::SUCCEEDED && !(positioner_.getResult()->at_location)) {
			ROS_INFO("Positioning -> Moving without ball. Reason: Marker lost!");
			setState(MOVING_WITHOUT_BALL);
		}
		break;
	}
	case(SEARCHING): {
		if (button_cancel_pressed_) {
			ROS_INFO("Canceling search!");
			if (grab_result_.ball_obtained) {
				ROS_INFO("Searching -> Moving with ball");
				setState(MOVING_WITH_BALL);
			} else {
				ROS_INFO("Searching -> Moving without ball");
				setState(MOVING_WITHOUT_BALL);
			}
		} else if (markerFinder_.getState() == GOALSTATE::SUCCEEDED && find_result_.marker_found) {
			ROS_INFO("Searching -> Positioning!");
			// Continue positioning.
			setState(POSITIONING);
		} else if (markerFinder_.getState() == GOALSTATE::SUCCEEDED && !(find_result_.marker_found)) {
			// No markers in range, cancel positioning.
			if (grab_result_.ball_obtained) {
				ROS_INFO("Searching -> Moving with ball! Reason: No markers in range!");
				setState(MOVING_WITH_BALL);
			} else {
				ROS_INFO("Searching -> Moving without ball! Reason: No markers in range!");
				setState(MOVING_WITHOUT_BALL);
			}
		}
		break;
	}
	case(GRABBING): {
		if (button_cancel_pressed_) {
			ROS_INFO("Grabbing -> Moving without ball!");
			setState(MOVING_WITHOUT_BALL);
		} else if (ballGrabber_.getState() == GOALSTATE::SUCCEEDED) {
			setState(MOVING_WITHOUT_BALL);
			if (ballGrabber_.getResult()) {
				ROS_INFO("Grabbing -> Moving with ball!");
				setState(MOVING_WITH_BALL);
				prepareEmotion(EMOTION_HAPPY, ros::Duration(3.0));
			} else {
				ROS_INFO("Grabbing -> Moving without ball!");
				setState(MOVING_WITHOUT_BALL);
				prepareEmotion(EMOTION_SAD, ros::Duration(3.0));
			}
		}
		break;
	}
	case(RELEASING): {
		if (button_cancel_pressed_) {
			// TODO Probably fails!
			ROS_INFO("Releasing -> Moving with ball!");
			setState(MOVING_WITH_BALL);
		} else if (ballDropper_.getState() == GOALSTATE::SUCCEEDED) {
			if (ballDropper_.getResult()) {
				ROS_INFO("Releasing -> Moving without ball!");
				setState(MOVING_WITHOUT_BALL);
			} else {
				ROS_INFO("Releasing -> Moving with ball!");
				setState(MOVING_WITH_BALL);
			}
		}
		break;
	}
	case(MANUAL): {
		if (!manual_busy_) {
			ROS_INFO("Manual -> Idle");
			setState(IDLE);
		}
		break;
	}
	default: {
		// Probably an error, return to idle mode.
		ROS_INFO("Errorstate -> Idle");
		setState(IDLE);
		break;
	}
	}
}

void StateMachine::prepareBallDrop() {
	if (!(dropper_busy_)) {
		goalCanceler();
		rp_states::DropBallGoal goal;
		goal.drop_ball = true;
		ballDropper_.sendGoal(goal,
				boost::bind(&StateMachine::ballDropperDoneCallback, this, _1, _2),
				boost::bind(&StateMachine::ballDropperActiveCallback, this));
	}
}
void StateMachine::prepareFindMarker() {
	if (!(finder_busy_)) {
		goalCanceler();
		rp_states::FindMarkerGoal goal;
		goal.marker_id = marker_current_;
		markerFinder_.sendGoal(goal,
				boost::bind(&StateMachine::markerFinderDoneCallback, this, _1, _2),
				boost::bind(&StateMachine::markerFinderActiveCallback, this));
	}
}
void StateMachine::prepareGrabBall() {
	if (!(grabber_busy_)) {
		goalCanceler();
		rp_states::GrabBallGoal goal;
		goal.marker_id = marker_ball_;
		ballGrabber_.sendGoal(goal,
				boost::bind(&StateMachine::ballGrabberDoneCallback, this, _1, _2),
				boost::bind(&StateMachine::ballGrabberActiveCallback, this));
	}
}
void StateMachine::prepareIdle() {
	if (!(idler_busy_)) {
		goalCanceler();
		rp_states::IdleGoal goal;
		goal.idle_duration = ros::Duration(5.0);
		idler_.sendGoal(goal,
				boost::bind(&StateMachine::idlerDoneCallback, this, _1, _2),
				boost::bind(&StateMachine::idlerActiveCallback, this));
	}
}
void StateMachine::preparePositionTarget() {
	if (!(positioner_busy_)) {
		goalCanceler();
		rp_states::PositionTargetGoal goal;
		goal.marker_id = marker_current_;
		ROS_INFO("Sending marker id: %i (In goal: %i)", marker_current_, goal.marker_id);
		positioner_.sendGoal(goal,
				boost::bind(&StateMachine::positionerDoneCallback, this, _1, _2),
				boost::bind(&StateMachine::positionerActiveCallback, this));
	}
}
void StateMachine::prepareDrive(bool has_ball) {
	if (!(driver_busy_)) {
		goalCanceler();
		rp_states::DriveGoal goal;
		goal.has_ball = has_ball;
		driver_.sendGoal(goal,
				boost::bind(&StateMachine::driverDoneCallback, this, _1, _2),
				boost::bind(&StateMachine::driverActiveCallback, this));
	}
}
void StateMachine::prepareManual() {
	if (!(manual_busy_)) {
		goalCanceler();
		rp_states::ManualControlGoal mGoal;
		mGoal.start		= true;

		manualController_.sendGoal(mGoal,
				boost::bind(&StateMachine::manualControlDoneCallback, this, _1, _2),
				boost::bind(&StateMachine::manualControlActiveCallback, this));
	}
	if (!(driver_busy_)) {
		rp_states::DriveGoal dGoal;
		dGoal.has_ball	= false;
		driver_.sendGoal(dGoal,
				boost::bind(&StateMachine::driverDoneCallback, this, _1, _2),
				boost::bind(&StateMachine::driverActiveCallback, this));
	}
}
inline void StateMachine::setPositionerTarget(unsigned int marker) {
	marker_current_ = marker;
}
inline int StateMachine::getPositionerTarget() {
	return marker_current_;
}
